#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys
import inspect
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
from SnakeRobot import SnakeRobot
import keyboard
import time
import _thread


robotlib = BwRobotLib()


#连接机器人设备
mlib = MiniBotControlLib("udp")
ip = "192.168.1.168"
port = 50000
mlib.Connect(ip, port, 2000) 

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"

#配置操作臂
snake = SnakeRobot(mlib)
snake.bindIds(robotlib.module_ids)

#-----------------------------------------------------------------------------
#仿生蛇
#-----------------------------------------------------------------------------


#参数设置
def handleSettingCmd(cmd):
    print("请输入参数并回车:")
    parameter = robotlib.inputNum()
   
    if(cmd == '6'):
        snake.setVelocity(parameter)
    if(cmd == '7'):
        snake.setSwingRange(parameter)
    if(cmd == '8'):
        snake.setSwingHz(parameter)

#处理键盘
def handleControlKeys(key):
    isDown = key.event_type == 'down'
    if not isDown:
        snake.stop()
        return

    cmd = key.name
    if not(cmd == '6' or cmd == '7' or cmd == '8' or cmd == '3'):
        if not (snake.stopMoving_signal):
            return

    if(cmd == '1'):
        snake.goForward()
    if(cmd == '2'):
        snake.goBack()
    if(cmd == '3'):
        snake.stop()
    if(cmd == '4'):
        snake.turnLeft()
    if(cmd == '5'):
        snake.turnRight()

    if(cmd == '6' or cmd == '7' or cmd == '8'):
        handleSettingCmd(cmd)
    pass


#控制提示
def controlTips():
    print("键盘控制命令 :")
    print("1 : 前进")
    print("2 : 后退")
    print("3 : 停止")
    print("4 : 左转")
    print("5 : 右转")


    print("-----------------------")
    print("0 :退出")
    
    keyboard.hook(handleControlKeys)
    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce
    #helper.Wait(True)
    keyboard.wait('0')
    keyboard.unhook_all()
    input("输入任意键回车（Enter）以继续.....")

#运行线程
def snakeRun(threadName,arg):
    snake.run()

print("输入任意字符 Enter 开始初始化...")
snake.stop()
_thread.start_new_thread(snakeRun,("runthread",snake))


while(1):
    print("命令：")
    print("1 : 控制")
    print("2 : 设置速率参数")
    print("3 : 设置振幅")
    print("4 : 设置频率")
    print("-----------------------")
    print("0 : 退出")
   
    cmd = robotlib.inputNum(int,"请输入：")
    if(cmd == 1):
       controlTips()
    if(cmd == 2):
       handleSettingCmd(6)
    if(cmd == 3):
       handleSettingCmd(7)
    if(cmd == 4):
       handleSettingCmd(8)
    if(cmd == 0):
       break

#while(1):
#    print("Keyboard to control :")
#    print("1 : Go Forward")
#    print("2 : Go Back")
#    print("3 : stop")
#    print("4 : Turn Left")
#    print("5 : Turn Right")
#    print("6 : Set Velocity ")
#    print("7 : Set Swing Range")
#    print("8 : Set Swing Hz")

#    print("-----------------------")
#    print("0 :Exit")

